///\todo upgrade

//////////////////////////////////////////////////////////////////////////////////
//                                                                              //
//  This file is part of the buola project (https://code.google.com/p/buola/).  //
//                                                                              //
//  Copyright(c) 2007-2012 Xavi Gratal                                          //
//  gratal AT gmail DOT com                                                     //
//                                                                              //
//  Buola is free software: you can redistribute it and/or modify               //
//  it under the terms of the GNU General Public License as published by        //
//  the Free Software Foundation, either version 3 of the License, or           //
//  (at your option) any later version.                                         //
//                                                                              //
//  Buola is distributed in the hope that it will be useful,                    //
//  but WITHOUT ANY WARRANTY; without even the implied warranty of              //
//  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the               //
//  GNU General Public License for more details.                                //
//                                                                              //
//  You should have received a copy of the GNU General Public License           //
//  along with buola.  If not, see <http://www.gnu.org/licenses/>.              //
//                                                                              //
//////////////////////////////////////////////////////////////////////////////////

#ifndef _BUOLA_MAT_CXRRT_H_
#define _BUOLA_MAT_CXRRT_H_

#include <buola/mat.h>
#include <buola/utility/random.h>

namespace buola { namespace mat {

template<typename tScalar>
class CXRRT
{
public:
    enum EType
    {
        POINT,
        LINE,
        TRIANGLE
    };

    typedef mat::CVec<tScalar> TPoint;

    struct SPrimitive
    {
        int mParent;
        EType mType;
        TPoint mPoint;
    };

    CXRRT()
    {}

    CXRRT(const TPoint &pInit)
    {
        mPrimitives.push_back({0,POINT,pInit});
    }

    CXRRT(const CXRRT&)=delete;
    CXRRT(CXRRT&&)=default;
    CXRRT &operator=(const CXRRT&)=delete;
    CXRRT &operator=(CXRRT&&)=delete;

    void AddPrimitive(const TPoint &pPoint,int pParent)
    {
        mPrimitives.push_back({pParent,POINT,pPoint});
        mPrimitives.push_back({pParent,LINE,pPoint});
        if(mPrimitives[pParent].mType==LINE)
        {
            mPrimitives.push_back({mPrimitives[pParent].mParent,LINE,pPoint});
            mPrimitives.push_back({pParent,TRIANGLE,pPoint});
        }
    }

    tScalar GetDistance(const TPoint &pPoint,int pI) const
    {
        const SPrimitive &lP=mPrimitives[pI];
        switch(lP.mType)
        {
        case POINT:
            return distance(pPoint,lP.mPoint);
        case LINE:
            {
                auto l1=lP.mPoint;
                auto l2=mPrimitives[lP.mParent].mPoint;
                return std::min(distance(pPoint,0.1*l1+0.9*l2),distance(pPoint,0.9*l1+0.1*l2));
            }
        case TRIANGLE:
        default:
            return INFINITY;
        }
    }

    int NearestNeighbour(const TPoint &pPoint) const
    {
        int lMin=0;
        auto lMinDist=GetDistance(pPoint,0);

        for(int i=1;i<mPrimitives.size();i++)
        {
            auto lDist=GetDistance(pPoint,i);
            if(lDist<lMinDist)
            {
                lMinDist=lDist;
                lMin=i;
            }
        }

        return lMin;
    }

    int size() const {   return mPrimitives.size();  }
    EType Type(int i) const {   return mPrimitives[i].mType;   }
    const TPoint &Point(int i) const {   return mPrimitives[i].mPoint;   }
    const TPoint &Parent(int i) const {   return mPrimitives[mPrimitives[i].mParent].mPoint;   }
    const TPoint &Parent2(int i) const {   return mPrimitives[mPrimitives[mPrimitives[i].mParent].mParent].mPoint;   }

    TPoint Center(int i)
    {
        const SPrimitive &lP=mPrimitives[i];
        switch(lP.mType)
        {
        case POINT:
            return lP.mPoint;
        case LINE:
        case TRIANGLE:
        default:
            return (lP.mPoint+mPrimitives[lP.mParent].mPoint)/2;
        }
    }
    
private:
    std::vector<SPrimitive> mPrimitives;
};

template<typename tScalar,typename tSpace>
CXRRT<tScalar> xrrt(const mat::CVec<tScalar> &pInit,int pK,tScalar pDelta,const tSpace &pSpace)
{
    CXRRT<tScalar> lRRT(pInit);

    for(int i=0;i<pK;i++)
    {
        auto lXRand=pSpace.RandomPoint();

        int lNN=lRRT.NearestNeighbour(lXRand);
        auto lCenter=lRRT.Center(lNN);
        mat::CVec<tScalar> lDiff=lXRand-lCenter;
        lDiff/=norm(lDiff);

        mat::CVec<tScalar> lNewPoint=lCenter+pDelta*lDiff;
        if(pSpace.Occupied(lNewPoint))
        {
            i--;
            continue;
        }

        lRRT.AddPrimitive(lNewPoint,lNN);
    }

    return lRRT;
}

/*namespace mat*/ } /*namespace buola*/ }

#endif
